%============================= lynx6_display =============================
%
%  function lynx6_display(alpha, ll)
%
%
%  Display the geometry associated with a specific lynx6 manipulator joint
%  configuration.
%
%============================= lynx6_display =============================

%
%  Name:	lynx6_display.m
%
%  Author:	Patricio A. Vela, pvela@gatech.edu
%
%  Created:	2008/10/16
%  Modified:	2008/10/16
%
%============================= lynx6_display =============================
function Roach_display(alpha, ll, viewparms)

if ( (nargin < 2) || isempty(ll) )
  ll = [1 0 0 1 2 3];
end

if ( (nargin < 3) )
  viewparms = [];
end
if (~isfield(viewparms,'view'))
  viewparms.view = {76, 30};
end

if (~isfield(viewparms, 'home'))
  viewparms.home = 'straight-down';
end

l6p.baserad = 95/(2*25.4);			% Radius of cylindrical base.


Rx = @(alpha)[ 1, 0, 0; 0, cos(alpha), -sin(alpha); 0, sin(alpha), cos(alpha)];
Ry = @(alpha)[cos(alpha), 0, sin(alpha); 0, 1, 0; -sin(alpha), 0, cos(alpha)];
Rz = @(alpha)[cos(alpha), -sin(alpha), 0; -sin(alpha), cos(alpha), 0; 0, 0, 1];

switch (viewparms.home)
  case 'straight-down'
    maxlen = sum(ll(1:end));

    g1 = [eye(3) , [0; ll(1); 0]; 0, 0, 0, 1];
    g2 = [Rz(alpha(1)) , [ll(2);0;0]; 0, 0, 0, 1];
    g3 = [Ry(alpha(2)) , [0;0;0]; 0, 0, 0, 1];
    g4 = [Ry(alpha(3)) , [0;0;-ll(3)]; 0, 0, 0, 1];
    g5 = [Ry(alpha(4)) , [0;0;-ll(4)]; 0, 0, 0, 1];
    g6 = [Rz(alpha(5)) , [0;0;0]; 0, 0, 0, 1];
    g7 = [eye(3) , [0; 0; -ll(5)]; 0, 0, 0, 1];
    
  case 'straight-out'
    maxlen = sum(ll(2:end));
    
    g1 = [eye(3) , [0; ll(1); 0]; 0, 0, 0, 1];
    g2 = [Rz(alpha(1)) , [ll(2);0;0]; 0, 0, 0, 1];
    g3 = [Ry(alpha(2)) , [0;0;0]; 0, 0, 0, 1];
    g4 = [Ry(alpha(3)) , [ll(3);0;0]; 0, 0, 0, 1];
    g5 = [Ry(alpha(4)) , [ll(4);0;0]; 0, 0, 0, 1];
    g6 = [Rx(alpha(5)) , [0;0;0]; 0, 0, 0, 1];
    g7 = [eye(3) , [ ll(5); 0; 0]; 0, 0, 0, 1];
    
end

lpts = [0;0;0];
lpts = [lpts, g1(1:3,4)];
gtemp = g1;
lpts = [lpts, gtemp(1:3,4)];
gtemp = gtemp*g2;
lpts = [lpts, gtemp(1:3,4)];
gtemp = gtemp*g3*g4;
lpts = [lpts, gtemp(1:3,4)];
gtemp = gtemp*g5;
lpts = [lpts, gtemp(1:3,4)];
gtemp = gtemp*g6*g7;
lpts = [lpts, gtemp(1:3,4)];
axfact = 15;

plot3(axfact*[-1 1],[0 0],[0 0],'b-.');
hold on;
plot3([0 0], axfact*[-1 1],[0 0],'b-.');
plot3([0 0], [0 0], axfact*[0 1],'b-.');
plot3(lpts(1,2:end-1), lpts(2,2:end-1), lpts(3,2:end-1)-gtemp(3,4), 'b.','MarkerSize',10);
plot3(lpts(1,:), lpts(2,:), lpts(3,:)-gtemp(3,4), 'r-','LineWidth',1);
hold off;

axis(.5*[-20, 20, -20, 20, 0, 10]);
view(viewparms.view{:});


end

%
%============================= lynx6_display =============================
